#ifndef NAVIGATION_TASK_H
#define NAVIGATION_TASK_H

#include "task/sync_task.h"
#include "utils/utils/date_time.h"
#include "robot_pose.h"
#include <mutex>

class NavigationTask: public task::SyncTask
{
public:
    explicit NavigationTask(const std::string &name)
        : task::SyncTask(name)
    {
    }
    ~NavigationTask()
    {
        EASE_INFO("~NavigationTask");
    }

private:
    bool onStart() override
    {
        EASE_INFO("name: %s, on start", task_name_.c_str());
        start_time_ = base::utils::DateTime::currentSecond();
        return true;
    }
    task::TaskStatus onRunning() override
    {
//        EASE_INFO("name: %s, on running", task_name_.c_str());
        if (is_cancel_)
        {
            EASE_INFO("on running, cancel task...");
            return task::Success;
        }
//        static int count = 0;
        int64_t now = base::utils::DateTime::currentSecond();
//        if (count++ == 20)
//        {
//            EASE_INFO("name: %s, on running", task_name_.c_str());
//            pause();
//            return task::Running;
//        }
        if (now - start_time_ <= cost_time_)
        {
//            EASE_INFO("name: %s, on running", task_name_.c_str());
//            count++;
            return task::Running;
        }
        EASE_INFO("name: %s, on running pass", task_name_.c_str());
        EASE_INFO("name: %s, running done", task_name_.c_str());

        RobotPose robotPose{};
        robotPose.setX(123.45);
        robotPose.setY(678.90);
        robotPose.setYaw(456.78);
        setContextData("pose", robotPose);
        return task::Success;
    }
    bool onPause() override
    {
        EASE_INFO("name: %s, on pause", task_name_.c_str());
        if (isPause())
        {
            EASE_INFO("is pause");
        }
        return true;
    }
    bool onResume() override
    {
        EASE_INFO("name: %s, on resume", task_name_.c_str());
        return true;
    }
    void onCancel() override
    {
        EASE_INFO("name: %s, on cancel", task_name_.c_str());
        is_cancel_ = true;
    }
    void onHalt() override
    {
        EASE_INFO("name: %s, on halt", task_name_.c_str());
    }
    void onDone(bool res) override
    {
        EASE_INFO("name: %s, on done, res: %s", task_name_.c_str(), res ? "pass" : "fail");
    }

    bool onReactive() override
    {
        EASE_INFO("name: %s, on reactive", task_name_.c_str());
        return true;
    }

private:
    int cost_time_{3};//s
    int64_t start_time_{};
    bool is_cancel_{false};
};

#endif //NAVIGATION_TASK_H
